
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       gp_spline_curve.c
  * @author     baiyang
  * @date       2022-9-6
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "gp_spline_curve.h"

#include <stdio.h>
#include <rtconfig.h>

#include <internal_error/internal_error.h>
/*-----------------------------------macro------------------------------------*/
#define SPLINE_FACTOR           4.0f    // defines shape of curves.  larger numbers result in longer spline curves, lower numbers take a direct path
#define TANGENTIAL_ACCEL_SCALER 0.5f    // the proportion of the maximum accel that can be used for tangential acceleration (aka in the direction of travel along the track)
#define LATERAL_ACCEL_SCALER    0.5f    // the proportion of the maximum accel that can be used for lateral acceleration (aka crosstrack acceleration)

// limit the maximum speed along the track to that which will achieve a cornering (aka lateral) acceleration of LATERAL_SPEED_SCALER * acceleration limit
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void slcurve_update_solution(spline_curve_t scurve, const Vector3f_t *origin, const Vector3f_t *dest, const Vector3f_t *origin_vel, const Vector3f_t *dest_vel);
static void slcurve_calc_dt_speed_max(spline_curve_t scurve, float time, float distance_delta, float *spline_dt, Vector3f_t *target_pos, Vector3f_t *spline_vel_unit, float *speed_max, float *accel_max);
static void slcurve_calc_target_pos_vel(spline_curve_t scurve, float time, Vector3f_t *position, Vector3f_t *velocity, Vector3f_t *acceleration, Vector3f_t *jerk);
/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void slcurve_ctor(spline_curve_t scurve)
{
    vec3_zero(&scurve->_origin);
    vec3_zero(&scurve->_destination);
    vec3_zero(&scurve->_origin_vel);
    vec3_zero(&scurve->_destination_vel);

    vec3_zero(&scurve->_hermite_solution[0]);
    vec3_zero(&scurve->_hermite_solution[1]);
    vec3_zero(&scurve->_hermite_solution[2]);
    vec3_zero(&scurve->_hermite_solution[3]);

    scurve->_time = 0.0f;
    scurve->_speed_xy = 0.0f;
    scurve->_speed_up = 0.0f;
    scurve->_speed_down = 0.0f;
    scurve->_accel_xy = 0.0f;
    scurve->_accel_z = 0.0f;
    scurve->_origin_speed_max = 0.0f;
    scurve->_destination_speed_max = 0.0f;

    scurve->_reached_destination = false;
    scurve->_zero_length = false;
}

// set maximum speed and acceleration
void slcurve_set_speed_accel(spline_curve_t scurve, float speed_xy, float speed_up, float speed_down,
                                  float accel_xy, float accel_z)
{
    scurve->_speed_xy = fabsf(speed_xy);
    scurve->_speed_up = fabsf(speed_up);
    scurve->_speed_down = fabsf(speed_down);
    scurve->_accel_xy = fabsf(accel_xy);
    scurve->_accel_z = fabsf(accel_z);
}

// set origin and destination using position vectors (offset from EKF origin)
// origin_vel is vehicle velocity at origin (in NEU frame)
// destination_vel is vehicle velocity at destination (in NEU frame)
// time is reset to zero
void slcurve_set_origin_and_destination(spline_curve_t scurve, const Vector3f_t *origin, const Vector3f_t *destination, const Vector3f_t *origin_vel, const Vector3f_t *destination_vel)
{
    // store origin and destination locations
    scurve->_origin = *origin;
    scurve->_destination = *destination;

    // handle zero length track
    scurve->_zero_length = math_flt_zero(vec3_sub_length_squared(destination, origin));
    if (scurve->_zero_length) {
        scurve->_time = 1.0f;
        vec3_zero(&scurve->_origin);
        vec3_zero(&scurve->_destination);
        scurve->_reached_destination = true;
        scurve->_origin_speed_max = 0.0f;
        scurve->_destination_speed_max = 0.0f;
        return;
    }

    scurve->_origin_vel = *origin_vel;
    scurve->_destination_vel = *destination_vel;
    scurve->_reached_destination = false;

    // reset time
    // Note: _time could include left-over from previous waypoint
    scurve->_time = 0.0f;

    // code below ensures we don't get too much overshoot when the next segment is short
    const float vel_len = vec3_length(&scurve->_origin_vel) + vec3_length(&scurve->_destination_vel);
    const float pos_len = vec3_sub_length(&scurve->_destination, &scurve->_origin) * SPLINE_FACTOR;
    if (vel_len > pos_len) {
        // if total start+stop velocity is more than four times the position difference
        // use a scaled down start and stop velocity
        const float vel_scaling = pos_len / vel_len;

        Vector3f_t origin_vel_scaling = {scurve->_origin_vel.x * vel_scaling,
                                         scurve->_origin_vel.y * vel_scaling,
                                         scurve->_origin_vel.z * vel_scaling};

        Vector3f_t destination_vel_scaling = {scurve->_destination_vel.x * vel_scaling,
                                              scurve->_destination_vel.y * vel_scaling,
                                              scurve->_destination_vel.z * vel_scaling};

        // update spline calculator
        slcurve_update_solution(scurve, &scurve->_origin, &scurve->_destination, &origin_vel_scaling, &destination_vel_scaling);
    } else {
        // update spline calculator
        slcurve_update_solution(scurve, &scurve->_origin, &scurve->_destination, &scurve->_origin_vel, &scurve->_destination_vel);
    }
    Vector3f_t target_pos = VECTOR3F_DEFAULT_VALUE;
    Vector3f_t spline_vel_unit = VECTOR3F_DEFAULT_VALUE;
    float spline_dt;
    float accel_max;
    slcurve_calc_dt_speed_max(scurve, 0.0f, 0.0f, &spline_dt, &target_pos, &spline_vel_unit, &scurve->_origin_speed_max, &accel_max);
    if (vec3_is_zero(&scurve->_destination_vel)) {
        scurve->_destination_speed_max = 0.0f;
    } else {
        slcurve_calc_dt_speed_max(scurve, 1.0f, 0.0f, &spline_dt, &target_pos, &spline_vel_unit, &scurve->_destination_speed_max, &accel_max);
    }
}

// move target location along track from origin to destination
// target_pos is updated with the target position from EKF origin in NEU frame
// target_vel is updated with the target velocity in NEU frame
void slcurve_advance_target_along_track(spline_curve_t scurve, float dt, Vector3f_t *target_pos, Vector3f_t *target_vel)
{
    // handle zero length track
    if (scurve->_zero_length) {
        *target_pos = scurve->_destination;
        vec3_zero(target_vel);
        return;
    }

    // calculate target position and velocity using spline calculator
    float speed_cms = vec3_length(target_vel);
    const float distance_delta = speed_cms * dt;
    float spline_dt;
    Vector3f_t spline_vel_unit = VECTOR3F_DEFAULT_VALUE;
    float speed_max;
    float accel_max;

    slcurve_calc_dt_speed_max(scurve, scurve->_time, distance_delta, &spline_dt, target_pos, &spline_vel_unit, &speed_max, &accel_max);
    speed_cms = math_constrain_float(speed_max, speed_cms - accel_max * dt, speed_cms + accel_max * dt);

    target_vel->x = spline_vel_unit.x * speed_cms;
    target_vel->y = spline_vel_unit.y * speed_cms;
    target_vel->z = spline_vel_unit.z * speed_cms;

    scurve->_time += spline_dt;

    // we will reach the destination in the next step so set reached_destination flag
    if (scurve->_time >= 1.0f) {
        scurve->_time = 1.0f;
        scurve->_reached_destination = true;
    }
}

// recalculate hermite_solution grid
//     relies on _origin_vel, _destination_vel and _origin and _destination
static void slcurve_update_solution(spline_curve_t scurve, const Vector3f_t *origin, const Vector3f_t *dest, const Vector3f_t *origin_vel, const Vector3f_t *dest_vel)
{
    scurve->_hermite_solution[0] = *origin;
    scurve->_hermite_solution[1] = *origin_vel;

    scurve->_hermite_solution[2].x = -origin->x*3.0f -origin_vel->x*2.0f + dest->x*3.0f - dest_vel->x;
    scurve->_hermite_solution[2].y = -origin->y*3.0f -origin_vel->y*2.0f + dest->y*3.0f - dest_vel->y;
    scurve->_hermite_solution[2].z = -origin->z*3.0f -origin_vel->z*2.0f + dest->z*3.0f - dest_vel->z;

    scurve->_hermite_solution[3].x = origin->x*2.0f + origin_vel->x -dest->x*2.0f + dest_vel->x;
    scurve->_hermite_solution[3].y = origin->y*2.0f + origin_vel->y -dest->y*2.0f + dest_vel->y;
    scurve->_hermite_solution[3].z = origin->z*2.0f + origin_vel->z -dest->z*2.0f + dest_vel->z;
}

// calculate the spline delta time for a given delta distance
// returns the spline position and velocity and maximum speed and acceleration the vehicle can travel without exceeding acceleration limits
static void slcurve_calc_dt_speed_max(spline_curve_t scurve, float time, float distance_delta, float *spline_dt, Vector3f_t *target_pos, Vector3f_t *spline_vel_unit, float *speed_max, float *accel_max)
{
    // initialise outputs
    *spline_dt = 0.0f;
    vec3_zero(spline_vel_unit);
    *speed_max = 0.0f;
    *accel_max = 0.0f;

    // calculate target position and velocity using spline calculator
    Vector3f_t spline_vel = VECTOR3F_DEFAULT_VALUE;
    Vector3f_t spline_accel = VECTOR3F_DEFAULT_VALUE;
    Vector3f_t spline_jerk = VECTOR3F_DEFAULT_VALUE;

    slcurve_calc_target_pos_vel(scurve, time, target_pos, &spline_vel, &spline_accel, &spline_jerk);

    // vel, accel and jerk should never all be zero
    if (vec3_is_zero(&spline_vel) && vec3_is_zero(&spline_accel) && vec3_is_zero(&spline_jerk)) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
        printf("SplineCurve::calc_dt_speed_max vel, accel and jerk are all zero\n");
#endif
        INTERNAL_ERROR(internal_error_invalid_arg_or_result);
        scurve->_reached_destination = true;
        return;
    }

    // aircraft velocity and acceleration along the spline will be defined based on the aircraft kinematic limits
    // aircraft velocity along the spline should be reduced to ensure normal accelerations do not exceed kinematic limits
    const float spline_vel_length = vec3_length(&spline_vel);
    if (math_flt_zero(spline_vel_length)) {
        // if spline velocity is zero then direction must be defined by acceleration or jerk
        if (math_flt_zero(vec3_length_squared(&spline_accel))) {
            // if acceleration is zero then direction must be defined by jerk
            vec3_norm(spline_vel_unit, &spline_jerk);
            *spline_dt = powf(6.0f * distance_delta / vec3_length(&spline_jerk), 1.0f/3.0f);
        } else {
            // all spline acceleration is in the direction of travel
            vec3_norm(spline_vel_unit, &spline_accel);
            *spline_dt = math_sqrtf(2.0f * distance_delta / vec3_length(&spline_accel));
        }
    } else {
        vec3_norm(spline_vel_unit, &spline_vel);
        *spline_dt = distance_delta / spline_vel_length;
    }

    // calculate acceleration normal to the direction of travel
    const float spline_accel_tangent_length = vec3_dot(&spline_accel, spline_vel_unit);
    Vector3f_t spline_accel_norm = {spline_accel.x - (spline_vel_unit->x * spline_accel_tangent_length),
                                    spline_accel.y - (spline_vel_unit->y * spline_accel_tangent_length),
                                    spline_accel.z - (spline_vel_unit->z * spline_accel_tangent_length)};

    const float spline_accel_norm_length = vec3_length(&spline_accel_norm);

    // limit the maximum speed along the track to that which will achieve a cornering (aka lateral) acceleration of LATERAL_SPEED_SCALER * acceleration limit
    const float tangential_speed_max = control_kinematic_limit(*spline_vel_unit, scurve->_speed_xy, scurve->_speed_up, scurve->_speed_down);
    const float accel_norm_max = LATERAL_ACCEL_SCALER * control_kinematic_limit(spline_accel_norm, scurve->_accel_xy, scurve->_accel_z, scurve->_accel_z);

    // sanity check to avoid divide by zero
    if (math_flt_zero(tangential_speed_max)) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
        printf("SplineCurve::calc_dt_speed_max tangential_speed_max is zero\n");
#endif
        INTERNAL_ERROR(internal_error_invalid_arg_or_result);
        scurve->_reached_destination = true;
        return;
    }

    if ((math_flt_positive(accel_norm_max)) && math_flt_positive(spline_accel_norm_length) && math_flt_positive(spline_vel_length) &&
         ((spline_accel_norm_length/accel_norm_max) > sq(spline_vel_length/tangential_speed_max))) {
        *speed_max = spline_vel_length / math_sqrtf(spline_accel_norm_length/accel_norm_max);
    } else {
        *speed_max = tangential_speed_max;
    }

    // calculate accel max and sanity check
    *accel_max = TANGENTIAL_ACCEL_SCALER * control_kinematic_limit(*spline_vel_unit, scurve->_accel_xy, scurve->_accel_z, scurve->_accel_z);
    if (math_flt_zero(*accel_max)) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
        printf("SplineCurve::calc_dt_speed_max accel_max is zero\n");
#endif
        INTERNAL_ERROR(internal_error_invalid_arg_or_result);
        scurve->_reached_destination = true;
        return;
    }
    const float dist = vec3_sub_length(&scurve->_destination, target_pos);
    *speed_max = MIN(*speed_max, math_sqrtf(2.0f * (*accel_max) * (dist + sq(scurve->_destination_speed_max) / (2.0f*(*accel_max)))));
}

// calculate target position and velocity from given spline time
// time is a value from 0 to 1
// position is updated with target position as an offset from EKF origin in NEU frame
// velocity is updated with the unscaled velocity
// relies on set_origin_and_destination having been called to update_solution
static void slcurve_calc_target_pos_vel(spline_curve_t scurve, float time, Vector3f_t *position, Vector3f_t *velocity, Vector3f_t *acceleration, Vector3f_t *jerk)
{
    const float time_sq = sq(time);
    const float time_cubed = time_sq * time;

    position->x = scurve->_hermite_solution[0].x + \
               scurve->_hermite_solution[1].x * time + \
               scurve->_hermite_solution[2].x * time_sq + \
               scurve->_hermite_solution[3].x * time_cubed;
    position->y = scurve->_hermite_solution[0].y + \
               scurve->_hermite_solution[1].y * time + \
               scurve->_hermite_solution[2].y * time_sq + \
               scurve->_hermite_solution[3].y * time_cubed;
    position->z = scurve->_hermite_solution[0].z + \
               scurve->_hermite_solution[1].z * time + \
               scurve->_hermite_solution[2].z * time_sq + \
               scurve->_hermite_solution[3].z * time_cubed;

    velocity->x = scurve->_hermite_solution[1].x + \
               scurve->_hermite_solution[2].x * 2.0f * time + \
               scurve->_hermite_solution[3].x * 3.0f * time_sq;
    velocity->y = scurve->_hermite_solution[1].y + \
               scurve->_hermite_solution[2].y * 2.0f * time + \
               scurve->_hermite_solution[3].y * 3.0f * time_sq;
    velocity->z = scurve->_hermite_solution[1].z + \
               scurve->_hermite_solution[2].z * 2.0f * time + \
               scurve->_hermite_solution[3].z * 3.0f * time_sq;

    acceleration->x = scurve->_hermite_solution[2].x * 2.0f + \
               scurve->_hermite_solution[3].x * 6.0f * time;
    acceleration->y = scurve->_hermite_solution[2].y * 2.0f + \
               scurve->_hermite_solution[3].y * 6.0f * time;
    acceleration->z = scurve->_hermite_solution[2].z * 2.0f + \
               scurve->_hermite_solution[3].z * 6.0f * time;

    jerk->x = scurve->_hermite_solution[3].x * 6.0f;
    jerk->y = scurve->_hermite_solution[3].y * 6.0f;
    jerk->z = scurve->_hermite_solution[3].z * 6.0f;
}

/*------------------------------------test------------------------------------*/


